#include "app_ctrl.h"

//  ================================================
//   内部使用函数声明
//  ================================================
uint8_t App_GetTargetNormalizedAngles(float* result);
void App_RestrictAngles(const float* in, float* out);
uint8_t App_WatchArmAngle(float* result);
uint8_t App_DriveMotorsOnce(float* target);

//  ================================================
//    外部可调用函数
//  ================================================

/**
 * @brief:    App_Init_Enable
 * @details:  初始化和使能函数
 */
motor_dm_t motor_1;
motor_dm_t motor_2;
motor_dm_t motor_3;
motor_dm_t motor_4;
motor_m2006_t motor_5;
motor_m2006_t motor_6;
uint16_t oid_enco_offset = 0;
uint16_t oid_neg_pi;
uint16_t oid_zero;
uint16_t oid_pos_pi;
uint8_t App_Init_Enable(void)
{
  /* 部件初始化 */
  BSP_CAN_Init();
  NRF24L01_Init();

  /* 电机初始化 */
  // 记录信息
  DM_Init(&motor_1, MOTOR_TX_ID_1, MOTOR_RX_ID_1, pos_mode);
  DM_Init(&motor_2, MOTOR_TX_ID_2, MOTOR_RX_ID_2, pos_mode);
  DM_Init(&motor_3, MOTOR_TX_ID_3, MOTOR_RX_ID_3, pos_mode);
  DM_Init(&motor_4, MOTOR_TX_ID_4, MOTOR_RX_ID_4, pos_mode);
  
  BSP_TIM_EnableElapsedCallback();
  
  // // 设0，减小偏移的计算量
  DM_ResetZero(&motor_1);
  DM_ResetZero(&motor_2);
  DM_ResetZero(&motor_3); 
  DM_ResetZero(&motor_4);

  // 使能电机
  HAL_Delay(30);
  DM_Enable(&motor_1);
  HAL_Delay(30);
  DM_Enable(&motor_2);
  HAL_Delay(30);
  DM_Enable(&motor_3);
  HAL_Delay(30);
  DM_Enable(&motor_4);
  HAL_Delay(30);

  uint16_t temp[6] = {0};
  NRF24L01_ReceiveAndUpdate(temp);
  oid_enco_offset = temp[3];
  
  // if (oid_enco_offset > 16383) {
  //   oid_neg_pi = oid_enco_offset - 16383;
  //   oid_zero = oid_enco_offset;
  //   oid_pos_pi = oid_enco_offset - 16383 - 1;
  // }
  // else if (oid_enco_offset < 16383) {
  //   oid_zero = oid_enco_offset;
  //   oid_pos_pi = oid_enco_offset + 16384;
  //   oid_neg_pi = oid_enco_offset + 16384 + 1;
  // }
  // else {
  //   oid_zero = oid_enco_offset;
  //   oid_pos_pi = oid_enco_offset + 16384;
  //   oid_neg_pi = oid_enco_offset - 16383;
  // }

  return 1;
}

/**
 * @brief:    App_ResetZeroData
 * @details:  设置零点，是针对控制器的偏移数据
 */
uint8_t App_ResetZeroData(uint16_t* controller_data, uint16_t* arm_data, uint16_t* total_data)
{
  uint8_t flag = NRF24L01_ReceiveAndUpdate(controller_data);
  for (uint8_t i = 0; i < 6; i++) {
    total_data[i + 0] = controller_data[i];
    total_data[i + 6] = arm_data[i];
  }
  return flag;
}

/**
 * @brief:    App_MainLoop
 * @details:  主函数的while中调用该函数
 * @related:  app_controller_angles   目标角度值，即控制器传递值
 *            app_arm_angles          机械臂当前角度值，正常工作状态下与前者一致
 */
float app_controller_angles[6]  = {0};
float app_ctrl_angles[6]        = {0};
float app_arm_angles[6]         = {0};
uint8_t App_MainLoop(void)
{
  App_GetTargetNormalizedAngles(app_controller_angles);
  App_RestrictAngles(app_controller_angles, app_ctrl_angles);
  App_WatchArmAngle(app_arm_angles);
  return App_DriveMotorsOnce(app_ctrl_angles);
}

//  ================================================
//    内部调用函数
//  ================================================

/**
 * @brief:    转化角度、加入限位逻辑
 * @details:  读取无线数据，将其转换为目标角度值
 */
uint8_t App_GetTargetNormalizedAngles(float* result)
{
  // 1. 读取编码值
  uint16_t raw[6];
  if (NRF24L01_ReceiveAndUpdate(raw) != 1) {
    return 0;
  };
  
  // 2. 处理偏移量 + 限位
  
  // 0
  if (raw[0] < 4095) {
    result[0] = ((float)raw[0]) / (float)(1<<13) * 2.0f * PI;
  } else if (raw[0] >= 4096) {
    result[0] = (float)((int)raw[0] - 8191) / (float)(1<<13) * 2.0f * PI;
  } else {
    // 不改变该角度
  }
  
  // 1, 2
  result[1] = uint_to_float(raw[1], -12.5, 12.5, 16);
  result[2] = uint_to_float(raw[2], -12.5, 12.5, 16);

  // 3
  if (oid_enco_offset < 16383) {
    // -pi -- x -- neg_num      (offset+16385 ~ 32767)
    if (raw[3] > (oid_enco_offset + 16384)) {
      result[3] = (float)((emax_15 - raw[3]) + oid_enco_offset) / (float)emax_15 * -2.0f * PI;
    }
    // zero -- x -- +pi         (offset ~ offset+16384)
    // neg_num -- x -- zero      (0 ~ offset-1)
    else {
      result[3] = (float)(raw[3] - oid_enco_offset) / (float)emax_15 * 2.0f * PI;
    }
  }
  else if (oid_enco_offset == 16383) {
    result[3] = (float)(raw[3] - oid_enco_offset) / (float)emax_15 * 2.0f * PI;
  }
  else {
    if (raw[3] < (oid_enco_offset - 16383)) {
      result[3] = (float)(emax_15 - oid_enco_offset + raw[3]) / (float)emax_15 * 2.0f * PI;
    }
    else {
      result[3] = (float)(raw[3] - oid_enco_offset) / (float)emax_15 * 2.0f * PI;
    }
  }

  return 1;
}

/**
 * @brief:    限位
 */
void App_RestrictAngles(const float* in, float* out)
{
  float agl_max[6] = {3.14f, 3.14f, 3.14f, 3.14f, 3.14f, 3.14f}; 
  float agl_min[6] = {-3.14f, -3.14f, -3.14f, -3.14f, -3.14f, -3.14f}; 
  
  for (uint8_t i = 0; i < 4; i++) {
    if (in[i] >= agl_max[i] || in[i] <= agl_min[i]) { } 
    else { out[i] = in[i]; }
  }
  
}

/**
 * @brief:    监视当前机械臂电机的角度
 */
uint8_t App_WatchArmAngle(float* result)
{
  DM_RequestFB(&motor_1);
  DM_RequestFB(&motor_2);
  DM_RequestFB(&motor_3);
  DM_RequestFB(&motor_4);
  HAL_Delay(1);

  uint16_t raw[6] = {0};
  raw[0] = 0x0000FFFF & ((((uint16_t)can_rx_data[0][1]) << 8) | can_rx_data[0][2]);
  raw[1] = 0x0000FFFF & ((((uint16_t)can_rx_data[1][1]) << 8) | can_rx_data[1][2]);
  raw[2] = 0x0000FFFF & ((((uint16_t)can_rx_data[2][1]) << 8) | can_rx_data[2][2]);
  raw[3] = 0x0000FFFF & ((((uint16_t)can_rx_data[3][1]) << 8) | can_rx_data[3][2]);

  for (uint8_t i = 0; i < 4; i++) {
    result[i] = uint_to_float(raw[i], -12.5, 12.5, 16);
  }

	return 1;
}

const float app_arm_max[6] = {-3.14f, -3.14f, -3.14f, -3.14f, -3.14f, -3.14f};
const float app_arm_min[6] = {3.14f, 3.14f, 3.14f, 3.14f, 3.14f, 3.14f};
uint8_t App_DriveMotorsOnce(float* target)
{
  float send_data[6];
  // 前四个
  // for (int i = 0; i < 4; i++) {
  //   if (target[i] > (app_arm_max[i] - 0.1f)) {      // 正向限幅
  //     send_data[i] = app_arm_max[i] - 0.1f;
  //   }
  //   else if (target[i] < (app_arm_min[i] + 0.1f)) { // 反向限幅
  //     send_data[i] = app_arm_min[i] + 0.1f;
  //   }
  //   else {
  //     send_data[i] = target[i];                     // 正常输出
  //   }
  // }

  send_data[0] = target[0];
  send_data[1] = target[1];
  send_data[2] = target[2];
  send_data[3] = target[3];

  // 5-6
  send_data[4] = send_data[5] = 0;

  // motor 1
  uint8_t flag = DM_DrivePOSCtrl(&hcan2, MOTOR_TX_ID_1, send_data[0], 3.0f);
  
  // motor 2
  flag &= DM_DrivePOSCtrl(&hcan2, MOTOR_TX_ID_2, send_data[1], 3.0f);

  // motor 3
  flag &= DM_DrivePOSCtrl(&hcan2, MOTOR_TX_ID_3, send_data[2], 3.0f);

  // motor 4
  flag &= DM_DrivePOSCtrl(&hcan2, MOTOR_TX_ID_4, send_data[3], 3.0f);
  
  // motor 5

  // motor 6


	return flag;
}
